package comunicacion.comandos;

import java.io.IOException;

import javax.vecmath.Point3d;

import comunicacion.Conector;
import comunicacion.ParametrosEntorno;
import comunicacion.vision.DoraemonServer;

import estrategia.ParametrosGlobales;

public class EntornoReal implements Conector {
	public static final int TIEMPO_ESPERA = 40;	
	private static final double FACTOR_VELOCIDAD = 10.0;
	private static final String NOMBRE_ARCHIVO_PROPIEDADES = "Parametros/EntornoReal.properties";
	
	private int idRobot;
	private String macRobot;
	private int motorIzquierdo;
	private int motorDerecho;
	
	private DoraemonServer doraemonServer;
		
	
	public EntornoReal(String ipServidorComandos, int puertoServidorComandos, String macRobot, int idMotorIzquierdo, int idMotorDerecho) throws IOException {
		this.idRobot = ParametrosGlobales.MI_ROBOT;
		this.macRobot = macRobot;
		this.motorIzquierdo = idMotorIzquierdo;
		this.motorDerecho = idMotorDerecho;
		
		// Establecemos la comunicacion con el servidor que recibe nuestros comandos 
		// y los envia al agente (via bluetooth)
		Sender.setServer(ipServidorComandos, puertoServidorComandos);
		
		// Establecemos la comunicacion con el modulo de vision
		doraemonServer = DoraemonServer.getInstance();
		// Inicializamos la recepcion de lecturas
		// TODO ver si esto no tiene que ir en alguna otra funcioncita, fuera del propio constructor
		doraemonServer.start();
		
	}	
	
	@Override
	public void detenerMotores() {		
		Sender.send(Commands.getAllMotorStop(macRobot));		
	}

	@Override
	public void enviarVelocidades(double velocidadIzquierda, double velocidadDerecha) {
		Sender.send(Commands.getMotorOutput(macRobot, motorIzquierdo, (int)(velocidadIzquierda*FACTOR_VELOCIDAD)));
		
		try {
			// Esperamos un tiempo antes de mandar la otra velocidad, para que no se pierda,
			// pues la experiencia demuestra que de no esperar, a menudo se dan situaciones raras
			Thread.sleep(TIEMPO_ESPERA);
		} catch (InterruptedException e) { }
		
		Sender.send(Commands.getMotorOutput(macRobot, motorDerecho, (int)(velocidadDerecha*FACTOR_VELOCIDAD)));
		
		try {
			// Esperamos un tiempo antes de mandar la otra velocidad, para que no se pierda,
			// pues la experiencia demuestra que de no esperar, a menudo se dan situaciones raras
			Thread.sleep(TIEMPO_ESPERA);
		} catch (InterruptedException e) { }
	}

	@Override
	public Point3d getCoordenadas() {
		//Point3d config = doraemonServer.getPosObject(idEntidad);
		Point3d config = doraemonServer.getPos();
		config = transformarCoordenadas(config);
		
		return config;
	}

	@Override
	public Point3d getCoordenadas(int idEntidad) {
		//Point3d config = doraemonServer.getPosObject(idEntidad);
		Point3d config = doraemonServer.getPosObject(idEntidad);
		config = transformarCoordenadas(config);
		
		return config;
	}
	
	@Override
	public Point3d[] getCoordenadasObjetos() throws IOException {
		Point3d[] posiciones = doraemonServer.getPosObject();
		
		for (int i=0; i<posiciones.length; i++) {
			if (posiciones[i] != null) 
				posiciones[i] = transformarCoordenadas(posiciones[i]);
		}
		
		return posiciones;
	}
	
	@Override
	public void enviarBeep() {
		Sender.send(Commands.getBeep(macRobot));
	}
	

	private Point3d transformarCoordenadas(Point3d config) {
		config.x = config.x / 1000.0;
		config.y = config.y / 1000.0;		
		config.z = Math.toRadians(config.z);
		
		return config;
	}

	public void cargarSimulacion(String path, String archivo) throws IOException {
		// TODO Auto-generated method stub
		
	}

	public void guardarSimulacion(String path, String archivo) throws IOException {
		// TODO Auto-generated method stub
		
	}
}
